package IdeaGui;

import org.apache.commons.math.linear.MatrixUtils;
import org.apache.commons.math.linear.RealMatrix;

/**
 * User: Mike
 * Date: 12/17/11
 */
public class KalmanFilter {
    private RealMatrix x;
    private RealMatrix p;

    public KalmanFilter(double[] initialStateEst, double[][] initialStateEstErrCov) {
        this.x = MatrixUtils.createColumnRealMatrix(initialStateEst);
        this.p = MatrixUtils.createRealMatrix(initialStateEstErrCov);
    }

    public void update(double[] obs, double[][] obsMatrix, double[][] obsCov, double[][] projection) {
        RealMatrix project = MatrixUtils.createRealMatrix(projection);
        RealMatrix h = project.multiply(MatrixUtils.createRealMatrix(obsMatrix));
        RealMatrix z = project.multiply(MatrixUtils.createColumnRealMatrix(obs));
        RealMatrix r = project.multiply(MatrixUtils.createRealMatrix(obsCov)).multiply(project.transpose());
        RealMatrix y = z.subtract(h.multiply(x));
        RealMatrix s = h.multiply(p.multiply(h.transpose())).add(r);
//        System.out.println(s);
        RealMatrix k = p.multiply(h.transpose().multiply(s.inverse()));
        x = x.add(k.multiply(y));
        p = ((MatrixUtils.createRealIdentityMatrix(k.getRowDimension()).subtract(k.multiply(h))).multiply(p));
    }

    public void update(double[] obs, double[][] obsMatrix, double[][] obsCov) {
        update(obs, obsMatrix, obsCov, MatrixUtils.createRealIdentityMatrix(obs.length).getData());
    }

    public void predict(double[][] transMatrix, double[][] transCov){
        RealMatrix f = MatrixUtils.createRealMatrix(transMatrix);
        RealMatrix q = MatrixUtils.createRealMatrix(transCov);
        x = f.multiply(x);
        p = f.multiply(p.multiply(f.transpose())).add(q);
    }

    public double[] getStateVector(){
        return x.getColumn(0);
    }

}
